import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from launch.substitutions import LaunchConfiguration
import launch.event_handlers
from launch_ros.parameter_descriptions import ParameterValue
import launch
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    pkg_share = get_package_share_directory('description_pkg')
    urdf_path = os.path.join(pkg_share, 'urdf', 'test.urdf.xacro')
    world = os.path.join(pkg_share,'worlds','empty.world')

    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    # 通过 IncludeLaunchDescription 包含另外一个 launch 文件
    launch_gazebo = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory(
            'gazebo_ros'), '/launch', '/gazebo.launch.py']),
      	# 传递参数
        launch_arguments={'use_sim_time': use_sim_time,'world':world,'verbose':'true'}.items()
    )
    # 为 Launch 声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', 
        default_value=str(urdf_path),
        description='URDF 的绝对路径')
    # 获取文件内容生成新的参数
    robot_description = ParameterValue(
        launch.substitutions.Command(
            ['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
    
    # 加载URDF模型到Gazebo
    # 请求 Gazebo 加载机器人
    spawn_entity_node = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        output='screen',
        arguments=['-topic', '/robot_description',
                   '-entity', 'agv_robot',
                   '-x', '0',
                   '-y', '0',
                   '-z', '0',
                   '-Y', '0',
                   ])
    
    robot_state_publisher_cmd = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name='robot_state_publisher',
        output='screen',
        parameters=[{
                'use_sim_time': use_sim_time,
                'robot_description': robot_description
        }],
    )

    load_joint_state_broadcaster = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'joint_state_controller'],
        output='screen'
    )

    load_arm_joint_position_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'arm_joint_position_controller'],
        output='screen'
    )

    ld = LaunchDescription()
    ld.add_action(launch_gazebo)
    ld.add_action(action_declare_arg_mode_path)
    ld.add_action(robot_state_publisher_cmd)
    ld.add_action(launch.actions.TimerAction(period=5.0,actions=[spawn_entity_node]))
    ld.add_action(launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=spawn_entity_node,
            on_exit=[load_joint_state_broadcaster],
        )
    ))
    ld.add_action(launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=load_joint_state_broadcaster,
            on_exit=[load_arm_joint_position_controller],
        )
    ))
    return ld
    
